﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using componentXtra;
using System.Drawing.Drawing2D;


namespace Simulador_Quadrotor
{
    public partial class ControlPIDForm : Form
    {

        #region Atributos

        PID pidAltura = null;
        PID pidRoll = null;
        PID pidPitch = null;
        PID pidYaw = null;
        List<double> tiempo = null;
        Dinamica dinamica = null;
        double tiempoTotal = 0;
        double tiempoFinal = 0;
        double tamanoPaso;
        double[] rot = null;
        Estado estadoFinal = null;
        Estado estadoActual = null;
        ControlMotor ControlMotor = null;
        ConfiguracionPurebaPID configuracionPurebaPID = null;

        #endregion

        #region Constructora

        public ControlPIDForm()
        {
            InitializeComponent();
            this.pidAltura = new PID();
            this.pidRoll = new PID();
            this.pidPitch = new PID();
            this.pidYaw = new PID();
            Estado estadoInicial = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);

            this.estadoActual = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            this.estadoFinal = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            rot = new double[] { 0, 0, 0, 0 };
            this.tiempo = new List<double>();
            this.dinamica = new Dinamica(rot, estadoInicial);
            this.ControlMotor = new ControlMotor();
        }
        #endregion


        #region Metodos

        /// <summary>
        /// Iniciar la simulacion del PID
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void toolStripButtonIniciar_Click(object sender, EventArgs e)
        {
            //Limpiar las graficas
            this.LimpiarSimulacionAnterior();
            //
            //double[] rotZ = new double[] { 0, 0, 0, 0 };
            //double[] rotPitch = new double[] { 0, 0, 0, 0 };
            //double[] rotYaw = new double[] { 0, 0, 0, 0 };
            //double[] rotRoll = new double[] { 0, 0, 0, 0 };
            //cargamos la configuracion
            this.tamanoPaso = (double)this.numericUpDownTPaso.Value;
            this.tiempoFinal = (double)this.numericUpDownTiempo.Value;
            this.estadoFinal.Z = (double)this.numericUpDownAlturaDeseada.Value;
            this.estadoFinal.phi = (double)this.numericUpDownRollDeseado.Value;
            this.estadoFinal.theta = (double)this.numericUpDownPitchDeseado.Value;
            this.estadoFinal.psi = (double)this.numericUpDownyawDeseado.Value;
            //configuramos el PID
            this.pidAltura.Kp = (double)this.numericUpDownAlturaP.Value;
            this.pidAltura.Kd = (double)this.numericUpDownAlturaD.Value;
            this.pidAltura.Ki = (double)this.numericUpDownAturaI.Value;
            this.pidAltura.Tp = this.tamanoPaso;

            this.pidRoll.Kp = (double)this.numericUpDownRollP.Value;
            this.pidRoll.Kd = (double)this.numericUpDownRollD.Value;
            this.pidRoll.Ki = (double)this.numericUpDownRollI.Value;
            this.pidRoll.Tp = this.tamanoPaso;

            this.pidPitch.Kp = (double)this.numericUpDownPitchP.Value;
            this.pidPitch.Kd = (double)this.numericUpDownPitchD.Value;
            this.pidPitch.Ki = (double)this.numericUpDownPitchI.Value;
            this.pidPitch.Tp = this.tamanoPaso;

            this.pidYaw.Kp = (double)this.numericUpDownYawP.Value;
            this.pidYaw.Kd = (double)this.numericUpDownYawD.Value;
            this.pidYaw.Ki = (double)this.numericUpDownYawI.Value;
            this.pidYaw.Tp = this.tamanoPaso;

            double controlZ;
            double controlPitch;
            double controlRoll;
            double controlYaw;

            //EMpezamos la simulacion
            int numeroPasos = 0;
            int.TryParse((this.tiempoFinal / this.tamanoPaso).ToString(), out numeroPasos);
            for (int i = 0; i < numeroPasos; i++)
            {
                this.tiempo.Add(tiempoTotal);
                //calculamos los estados
                this.estadoActual = this.dinamica.IntegraPaso(ref this.tiempoTotal, this.tamanoPaso, this.rot);
                //controlamos la altura
                controlZ = this.pidAltura.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.Z - this.estadoActual.Z);
                //controlamos el Roll
                controlRoll = this.pidRoll.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.phi - this.estadoActual.phi);
                //controlamos el Pitch              
                controlPitch = this.pidPitch.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.theta - this.estadoActual.theta);
                //controlamos el Yaw
                controlYaw = this.pidYaw.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.psi - this.estadoActual.psi);
                //controlamos las salidas de los rotores
                this.rot = this.ControlMotor.Controlar(controlZ, controlRoll, controlPitch, controlYaw, this.estadoFinal.Z - this.estadoActual.Z);
                //Pintamos los valores
                this.PintaGrafica(GraficaAltura, this.tiempoTotal, this.estadoActual.Z);
                this.PintaGrafica(GraficaRoll, this.tiempoTotal, this.estadoActual.phi);
                this.PintaGrafica(GraficaPitch, this.tiempoTotal, this.estadoActual.theta);
                this.PintaGrafica(GraficaYaw, this.tiempoTotal, this.estadoActual.psi);
                this.PintaGrafica(GraficaRotor1, this.tiempoTotal, rot[0]);
                this.PintaGrafica(GraficaRotor2, this.tiempoTotal, rot[1]);
                this.PintaGrafica(GraficaRotor3, this.tiempoTotal, rot[2]);
                this.PintaGrafica(GraficaRotor4, this.tiempoTotal, rot[3]);

            }
            MessageBox.Show("x  " + this.estadoActual.X + "  ,  y" + this.estadoActual.Y);
        }


        /// <summary>
        /// Fubcion que pinta los resultados ne la grafica
        /// </summary>
        private void PintaGrafica(XYGraph grafica, List<double> variable)
        {
            grafica.ClearGraphs();
            grafica.AddGraph("", DashStyle.Solid, Color.Red, 1, false);
            for (int i = 0; i < variable.Count; i++)
            {
                grafica.AddValue(0, (float)this.tiempo[i], (float)variable[i]);
            }
            grafica.DrawAll();
        }

        private void PintaGrafica(XYGraph grafica, double tiempo, double valor)
        {

            grafica.AddGraph("", DashStyle.Solid, Color.Red, 0, false);
            grafica.AddValue(0, (float)tiempo, (float)valor);
            grafica.DrawAll();
        }

        private void LimpiarSimulacionAnterior()
        {
            Estado estadoInicial = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            this.estadoActual = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            this.estadoFinal = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            this.rot = new double[] { 0, 0, 0, 0 };
            this.tiempo.Clear();
            this.tiempoTotal = 0;
            this.dinamica = new Dinamica(rot, estadoInicial);
            this.GraficaAltura.ClearGraphs();
            this.GraficaPitch.ClearGraphs();
            this.GraficaRoll.ClearGraphs();
            this.GraficaYaw.ClearGraphs();
            this.GraficaRotor1.ClearGraphs();
            this.GraficaRotor2.ClearGraphs();
            this.GraficaRotor3.ClearGraphs();
            this.GraficaRotor4.ClearGraphs();
            this.pidAltura = new PID();
            this.pidPitch = new PID();
            this.pidRoll = new PID();
            this.pidYaw = new PID();
        }

        #endregion

        private void toolStripButtonGuardar_Click(object sender, EventArgs e)
        {
            GuardarConfiguracion();

        }

        private void GuardarConfiguracion()
        {
            this.saveFileDialog1.Filter = "Configuracion PID (*.cfPID)|*.cfPID|Todos los archivos (*.*)|*.*";
            this.saveFileDialog1.Title = "Guardar configuracion del controlador";
            this.saveFileDialog1.AddExtension = true;
            if (this.saveFileDialog1.ShowDialog() == DialogResult.OK)
            {
                Guardar();
            }
        }

        private void Guardar()
        {
            if (configuracionPurebaPID == null)
            {
                configuracionPurebaPID = new ConfiguracionPurebaPID()
                {
                    Altura = new DatosPID()
                    {
                        kd = (double)this.numericUpDownAlturaD.Value,
                        ki = (double)this.numericUpDownAturaI.Value,
                        kp = (double)this.numericUpDownAlturaP.Value
                    },
                    Pitch = new DatosPID()
                    {
                        kd = (double)this.numericUpDownPitchD.Value,
                        ki = (double)this.numericUpDownPitchI.Value,
                        kp = (double)this.numericUpDownPitchP.Value
                    },
                    Roll = new DatosPID()
                    {
                        kd = (double)this.numericUpDownRollD.Value,
                        ki = (double)this.numericUpDownRollI.Value,
                        kp = (double)this.numericUpDownRollP.Value
                    },
                    Yaw = new DatosPID()
                    {
                        kd = (double)this.numericUpDownYawD.Value,
                        ki = (double)this.numericUpDownYawI.Value,
                        kp = (double)this.numericUpDownYawP.Value
                    },
                    X = new DatosPID()
                    {
                        kd = 5,
                        ki = 0.0001,
                        kp = 4
                    },
                    Y = new DatosPID()
                    {
                        kd = 5,
                        ki = 0.0001,
                        kp = 4
                    },
                    AlturaDeseada = (double)this.numericUpDownAlturaDeseada.Value,
                    PitchDeseada = (double)this.numericUpDownPitchDeseado.Value,
                    RollDeseada = (double)this.numericUpDownRollDeseado.Value,
                    YawDeseada = (double)this.numericUpDownyawDeseado.Value,
                    Tiempo = (double)this.numericUpDownTiempo.Value,
                    TPaso = (double)this.numericUpDownTPaso.Value

                };

            }
            string path = this.saveFileDialog1.FileName;
            Serializador serializador = new Serializador(path);
            serializador.SerializarXML(typeof(ConfiguracionPurebaPID), configuracionPurebaPID);
        }

        private void toolStripButtonCargar_Click(object sender, EventArgs e)
        {
            this.openFileDialog1.AddExtension = true;
            this.openFileDialog1.DefaultExt = "Configuracion PID (*.cfPID)|*.cfPID|Todos los archivos (*.*)|*.*";
            this.openFileDialog1.Filter = "Configuracion PID (*.cfPID)|*.cfPID|Todos los archivos (*.*)|*.*";
            this.openFileDialog1.Title = "Cargar configuracion de PID";
            if (this.openFileDialog1.ShowDialog() == DialogResult.OK)
            {
                string path = this.openFileDialog1.FileName;
                Serializador serializador = new Serializador(path);
                ConfiguracionPurebaPID configuracionPurebaPID = serializador.DeserializarXML(typeof(ConfiguracionPurebaPID)) as ConfiguracionPurebaPID;
                this.numericUpDownAlturaDeseada.Value = (decimal)configuracionPurebaPID.AlturaDeseada;
                this.numericUpDownAlturaD.Value = (decimal)configuracionPurebaPID.Altura.kd;
                this.numericUpDownAlturaP.Value = (decimal)configuracionPurebaPID.Altura.kp;
                this.numericUpDownAturaI.Value = (decimal)configuracionPurebaPID.Altura.ki;

                this.numericUpDownPitchDeseado.Value = (decimal)configuracionPurebaPID.PitchDeseada;
                this.numericUpDownPitchD.Value = (decimal)configuracionPurebaPID.Pitch.kd;
                this.numericUpDownPitchP.Value = (decimal)configuracionPurebaPID.Pitch.kp;
                this.numericUpDownPitchI.Value = (decimal)configuracionPurebaPID.Pitch.ki;

                this.numericUpDownRollDeseado.Value = (decimal)configuracionPurebaPID.RollDeseada;
                this.numericUpDownRollD.Value = (decimal)configuracionPurebaPID.Roll.kd;
                this.numericUpDownRollP.Value = (decimal)configuracionPurebaPID.Roll.kp;
                this.numericUpDownRollI.Value = (decimal)configuracionPurebaPID.Roll.ki;

                this.numericUpDownyawDeseado.Value = (decimal)configuracionPurebaPID.YawDeseada;
                this.numericUpDownYawD.Value = (decimal)configuracionPurebaPID.Yaw.kd;
                this.numericUpDownYawP.Value = (decimal)configuracionPurebaPID.Yaw.kp;
                this.numericUpDownYawI.Value = (decimal)configuracionPurebaPID.Yaw.ki;

                this.numericUpDownTPaso.Value = (decimal)configuracionPurebaPID.TPaso;
                this.numericUpDownTiempo.Value = (decimal)configuracionPurebaPID.Tiempo;
                this.configuracionPurebaPID = configuracionPurebaPID;
            }
        }

        private void toolStripButtonSegundoPaso_Click(object sender, EventArgs e)
        {
            PIDxy pIDxy = new PIDxy(this.configuracionPurebaPID);
            if (pIDxy.ShowDialog() == DialogResult.OK)
            {
                this.GuardarConfiguracion();
            }
        }

    }
}
